import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.File;
import java.io.IOException;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.MotorPort;
import lejos.nxt.NXTRegulatedMotor;
import lejos.nxt.Sound;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;
import lejos.nxt.comm.NXTConnection;
import lejos.util.Delay;

public class Blue
{

	/**
	 * @param args
	 */
	public static void main(String[] args)
	{
		boolean finished = false;
		NXTRegulatedMotor motorA = new NXTRegulatedMotor(MotorPort.A);
		NXTRegulatedMotor motorB = new NXTRegulatedMotor(MotorPort.B);
		NXTRegulatedMotor motorC = new NXTRegulatedMotor(MotorPort.C);
		 motorA.setSpeed(30);
		 motorB.setSpeed(20);
		LCD.drawString("Waiting for connection...", 0, 1);
		BTConnection btc = Bluetooth.waitForConnection(30000, NXTConnection.RAW);
		LCD.drawString("                        ", 0, 1);
		if (btc == null)
		{
			return;
		}
		btc.setIOMode(NXTConnection.RAW);
		DataInputStream dis = btc.openDataInputStream();
		DataOutputStream dos = btc.openDataOutputStream();
		LCD.drawString("                        ", 0, 1);
		LCD.drawString("Connected! :)", 0, 1);
//		try
//		{
//			if (dis.available() < 0)
//			{
//				return;
//			}
//		}
//		catch (IOException e1)
//		{
//			LCD.drawString("                        ", 0, 1);
//			LCD.drawString("died", 0, 1);
//			btc.closeStream();
//			btc.close();
//			// e1.printStackTrace();
//		}
		while (!Button.ESCAPE.isPressed() && !finished)
		{
			try
			{
				int command = dis.readInt();
				int value = dis.readInt();

				int positionUP = motorB.getPosition();
				LCD.drawString(command + " " + value, 0, 3);
				if (command == 1 || command == 2 || command == -1)
				{
					motorA.rotate(value * 5, true);

				}
				LCD.drawString("motorB pos: " + positionUP, 0, 5);

				if (command == 3) // down
				{
					if (positionUP < 1)
					{
						motorB.rotate(Math.abs(value * 2), true);
					}
				}
				else if (command == 4)// up
				{
					if (positionUP > -45)
					{
						motorB.rotate(-Math.abs(value * 2), true);
					}
				}
				
				if(command == 5) //fire
				{
					motorC.rotate(-Math.abs(value), true);					
					finished = true;
				}

				if (command == 99 && value == 99)
				{
					finished = true;
				}
			}
			catch (Exception e)
			{
				// TODO Auto-generated catch block
				LCD.drawString("                  ", 0, 1);
				// LCD.drawString("Died :(", 0, 1);
				LCD.drawString(e.getMessage(), 0, 1);
				// e.printStackTrace();
			}
		}
		btc.closeStream();
		btc.close();
	}

}
